#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import argparse
# import wiringpi

import numpy as np
import sys

import serial  # 导入模块
import serial.tools.list_ports
import threading
import struct
import time
import csv
import os
import psutil

import platform
from CRC import checkCRC16, checkCRC8
# from copy import deepcopy
# import sys
# import os
# import math

# from serial import EIGHTBITS, PARITY_NONE, STOPBITS_ONE

# 宏定义参数
PI = 3.1415926
FRAME_HEAD = 0xfc
FRAME_END = 0xfd

TYPE_IMU = 0x40
TYPE_AHRS = 0x41
TYPE_INSGPS = 0x42

TYPE_GEODETIC_POS = 0x5c #纬度经度高度
TYPE_GROUND = 0xf0
TYPE_SYS_STATE = 0x50 #海拔
TYPE_BODY_ACCELERATION = 0x62 #机体xyz轴加速度+重力加速度
TYPE_ACCELERATION = 0x61 #机体xyz轴加速度
IMU_LEN = 0x38  # //56
AHRS_LEN = 0x30  # //48
INSGPS_LEN = 0x48  # //72
GEODETIC_POS_LEN = 0x20  # //32
SYS_STATE_LEN = 0x64  # // 100
BODY_ACCELERATION_LEN = 0x10#// 16
ACCELERATION_LEN = 0x0c  # 12
PI = 3.141592653589793
DEG_TO_RAD = 0.017453292519943295
isrun = True



# 获取命令行输入参数，这里的串口default需要修改，win用com5,linux用tty
def parse_opt(known=False):
    parser = argparse.ArgumentParser()
    # parser.add_argument('--debugs', type=bool, default=False, help='if debug info output in terminal ')
    parser.add_argument('--port', type=str, default='/dev/ttyUSB0', help='the models serial port receive data; example: '
                                                                 '    Windows: COM5'
                                                                 '    Linux: /dev/ttyUSB1')

    parser.add_argument('--bps', type=int, default=921600, help='the models baud rate set; default: 115200')
    parser.add_argument('--timeout', type=int, default=20, help='set the serial port timeout; default: 20')
    # parser.add_argument('--device_type', type=int, default=0, help='0: origin_data, 1: for single imu or ucar in ROS')

    receive_params = parser.parse_known_args()[0] if known else parser.parse_args()
    return receive_params

# 数据存储
Datafolder = 'Data/IMUData/'
if not os.path.exists(Datafolder):
        os.makedirs(Datafolder)
formatted_time = time.strftime('%Y%m%d%H%M%S',time.localtime())
IMU_data_file = Datafolder + f"IMU_data_{formatted_time}.csv"
AHRS_data_file = Datafolder + f"AHRS_data_{formatted_time}.csv"
INSGPS_data_file = Datafolder + f"INSGPS_data_{formatted_time}.csv"
GPS_data1_file = Datafolder + f"GPS_data_{formatted_time}.csv"
GPS_data2_file = Datafolder + f"GPS_data_{formatted_time}.csv"
GPS_data3_file = Datafolder + f"GPS_data_{formatted_time}.csv"
GPS_data4_file = Datafolder + f"GPS_data_{formatted_time}.csv"



# 线程锁
lock = threading.Lock()

def checkPackSize(head_type, check_len):
    if head_type == TYPE_IMU and check_len != IMU_LEN:
        return False
    elif head_type == TYPE_AHRS and check_len != AHRS_LEN:
        return False
    elif head_type == TYPE_INSGPS and check_len != INSGPS_LEN:
        return False
    elif head_type == TYPE_GEODETIC_POS and check_len != GEODETIC_POS_LEN:
        return False
    elif head_type == TYPE_SYS_STATE and check_len != SYS_STATE_LEN:
        return False
    elif head_type == TYPE_GROUND or head_type == 0x50:
        return False
    elif head_type == TYPE_BODY_ACCELERATION and check_len != BODY_ACCELERATION_LEN:
        print("check head type "+str(TYPE_BODY_ACCELERATION)+" failed;"+" check_LEN:"+str(check_len))
        return False
    elif head_type == TYPE_ACCELERATION and check_len != ACCELERATION_LEN:
        print("check head type "+str(TYPE_ACCELERATION)+" failed;"+" ckeck_LEN:"+str(check_len))
        return False
    else :
        return True

TempBytes = []         # 临时数据列表

def DecodeData(data):
    # timestamp = time.time()
    # print("check_len_time:", time.time())
    
    global TempBytes
    for val in data:
            TempBytes.append(val)
            # 校验帧头
            if (TempBytes[0] != FRAME_HEAD):                   #非标识符0x55开头的
                # print("No head!,is 0x{:X}".format(TempBytes[0]))
                del TempBytes[0]                       #去除第一个字节
                continue
            
            # 校验数据类型
            if (len(TempBytes) > 1):
                head_type = TempBytes[1]
                if (head_type != TYPE_IMU and head_type != TYPE_AHRS and head_type != TYPE_INSGPS and
                        head_type != TYPE_GEODETIC_POS and head_type != 0x50 and head_type != TYPE_GROUND and
                        head_type != TYPE_SYS_STATE and head_type!=TYPE_BODY_ACCELERATION and head_type!=TYPE_ACCELERATION):
                    # print("head_type is err!,is 0x{:X}".format(head_type))
                    del TempBytes[0]
                    continue
            if (len(TempBytes) > 2):
                payloadSize = TempBytes[2]
                head_type = TempBytes[1]
                # 校验字长对不对、数据完不完整
                if not checkPackSize(head_type, payloadSize):
                    del TempBytes[0]
                    continue
                else:
                    pass
                if(len(TempBytes) >= payloadSize + 8):
                    # if(len(TempBytes) != payloadSize + 8): # TempBytes[-1] == FRAME_END
                    #     print("缺少数据")
                    #     del TempBytes[0]                    # 去除第一个字节
                    #     continue
                    # else:
                    #     pass
                    serialNum = TempBytes[3] # 流水号，没啥用
                    head_crc8 = TempBytes[4] # 数据头的crc校验
                    crc16_H_s = TempBytes[5]
                    crc16_L_s = TempBytes[6]
                    if ((checkCRC16(TempBytes[7:7 + payloadSize]) == (crc16_H_s << 8)|crc16_L_s) and checkCRC8(TempBytes[0:4]) == head_crc8):
                        data_s = bytes(TempBytes[7:7 + payloadSize])
                        if TempBytes[1] == TYPE_IMU:
                            #print("拆包")
                            IMU_DATA = struct.unpack('12f ii',data_s[0:56])
                            timestamp = time.time()
                            IMU_DATA = np.append(IMU_DATA, timestamp)
                            #print("IMU1收到数据")
                            with lock:
                                with open(IMU_data_file, "a") as f:
                                    writer = csv.writer(f)
                                    writer.writerow(IMU_DATA)
                                    #print("写入完毕")

                        elif TempBytes[1] == TYPE_AHRS:           #返回读取指定的寄存器
                            AHRS_DATA = struct.unpack('10f ii',data_s[0:48])
                            timestamp = time.time()
                            AHRS_DATA = np.append(AHRS_DATA, timestamp)

                            with lock:
                                with open(AHRS_data_file, "a") as f:
                                    writer = csv.writer(f)
                                    writer.writerow(AHRS_DATA)
                        
                        elif TempBytes[1]  == TYPE_INSGPS:
                            INSGPS_DATA = struct.unpack('16f ii',data_s[0:72])
                            timestamp = time.time()
                            INSGPS_DATA = np.append(INSGPS_DATA, timestamp)

                            with lock:
                                with open(INSGPS_data_file, "a") as f:
                                    writer = csv.writer(f)
                                    writer.writerow(INSGPS_DATA)
                        TempBytes=[]                        #清除数据
                    else:                                   #校验和未通过
                        # print("check is wrong!")
                        # if (checkCRC16(TempBytes[7:7 + payloadSize]) != (crc16_H_s << 8)|crc16_L_s):
                        #     print(' '.join(['{:02X}'.format(x) for x in TempBytes[7:7 + payloadSize]]))
                        #     print('{:04X}'.format(checkCRC16(TempBytes[7:7 + payloadSize])), '{:04X}'.format((crc16_H_s << 8)|crc16_L_s))
                        # print(' '.join(['{:02X}'.format(x) for x in TempBytes]))
                        del TempBytes[0]                    # 去除第一个字节
                else:
                    pass
                    # print(checkPackSize(TempBytes[1], TempBytes[2]) , len(TempBytes) == TempBytes[2] - 8)
                    # if(TempBytes[-1] == FRAME_END):
                    #     print(' '.join(['{:02X}'.format(x) for x in TempBytes]))
                    # del TempBytes[0]                    # 去除第一个字节


# 接收数据线程
def receive_data():
    # open_port()
    # find_serial()

    # 尝试打开串口
    try:
        # serial_ = serial.Serial(port=opt.port, baudrate=opt.bps, bytesize=EIGHTBITS, parity=PARITY_NONE,
        #                         stopbits=STOPBITS_ONE,
        #                         timeout=opt.timeout)
        serial_ = serial.Serial('/dev/ttyUSB0', 921600, timeout=20)  # 请根据实际情况替换串口名和波特率,com7

        # print("baud rates = " + str(serial_.baudrate))
        print("IMU connect")
    except:
        print("error:  unable to open port .")
        sys.exit(1)

    # 先清空buffer
    serial_.reset_input_buffer()
    # 循环读取数据
    while serial_.isOpen() and tr.is_alive() :
        try:
            tlen = serial_.inWaiting()
            if (tlen>0):
                data = serial_.read(tlen)
                DecodeData(data)
        except Exception as ex:
            print(ex)
        # 查看线程是否还在运行
        if not threading.main_thread().is_alive():
            print('done')
            break
        


# # 寻找输入的port串口
# def find_serial():
#     port_list = list(serial.tools.list_ports.comports())
#     for port in port_list:
#         if port.device == opt.port:
#             return True
#     return False


# def open_port():
#     if find_serial():
#         print("find this port : " + opt.port)
#     else:
#         print("error:  unable to find this port : " + opt.port)
#         sys.exit(1)

def write_file():
    with open(IMU_data_file, "a") as f:
        writer = csv.writer(f)
        writer.writerow(["Gyroscope_X","Gyroscope_Y","Gyroscope_Z","Accelerometer_X","Accelerometer_Y","Accelerometer_Z",\
                         "Magnetometer_X","Magnetometer_Y","Magnetometer_Z","IMU_Temp","Pressure","Pre_Temp","Timestamp","None","Time"])
    with open(AHRS_data_file, "a") as f:
        writer = csv.writer(f)
        writer.writerow(["RollSpeed","PitchSpeed","HeadingSpeed","Roll","Pitch","Heading","Q1","Q2","Q3","Q4","Timestamp","None","Time"])

# 进程查询，防止多运行
def checkProcess(processName):
    # processName = os.path.basename(__file__)
    ps1=psutil.pids()
    curps=os.getpid()
    for pid in ps1:
        try:
            cmdline = psutil.Process(pid).cmdline()
        except psutil.NoSuchProcess:
            continue
        # print(cmdline)
        if processName in cmdline and pid != curps and pid != (curps - 1):
            print(f"{processName}:, This is the second process pid = {str(pid)}, so exit!")
            sys.exit(1)
    print(f"now runing {processName},current pid = "+str(curps))
    return

if __name__ == "__main__":
    # wiringpi.wiringPiSetup()
    # print(wiringpi.micros())
    _fileName = sys.argv[1]
    checkProcess(_fileName)
    write_file()
    # opt = parse_opt()
    tr = threading.Thread(target=receive_data)
    tr.start()
    while True:
        try:
            if tr.is_alive():
                time.sleep(1000)
                # print("主线程")
            else:
                break
        except(KeyboardInterrupt, SystemExit):
            break
